#include "fixed_point_camera.h"

FixedPointCamera::FixedPointCamera(Point at,Point eye,Point up,float fovUp,float aspect,float dNear,float dFar)
	: PerspectiveCamera(eye,at,up,fovUp,aspect,dNear,dFar){
	Point dir = at-eye;
	xytheta = atan2(dir.x1,dir.x2);
	yztheta = atan2(sqrt(pow(dir.x1,2)+pow(dir.x2,2)),dir.x3);
}

FixedPointCamera::~FixedPointCamera(){
}


void FixedPointCamera::applyModelViewTransformations(){
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	Point eye = this->getEye();
	Point at = this->getAt();
	Point up = this->getUp();
	gluLookAt(eye.x1, eye.x2, eye.x3, at.x1, at.x2, at.x3, up.x1, up.x2, up.x3);
}

void FixedPointCamera::moveTo(const Point& p){
	setFixedPoint(p);
}

void FixedPointCamera::move(float offset){
	if(offset<0)
		offset=-1/offset;

	Point at = this->getAt();

	Matrix4x4 t1(-at.x1,-at.x2,-at.x3);
	Matrix4x4 e1(offset,offset,offset,true);
	Matrix4x4 t2(at.x1,at.x2,at.x3);
	Matrix4x4 t;
	t *= t1;
	t *= e1;
	t *= t2;

	frustum.applyTransformation(t);
}

void FixedPointCamera::rotateUD(float theta){
	Point eye = this->getEye();
	Point at = this->getAt();
	Matrix4x4 m(-at.x1,-at.x2,-at.x3);
	Matrix4x4 r1(-xytheta,Point(0,0,1));
	Matrix4x4 r2(-yztheta,Point(1,0,0));
	Matrix4x4 r3(theta,Point(1,0,0));
	Matrix4x4 r4(yztheta,Point(1,0,0));
	Matrix4x4 r5(xytheta,Point(0,0,1));
	Matrix4x4 t2(at.x1,at.x2,at.x3);
	m *= r1;
	m *= r2;
	m *= r3;
	m *= r4;
	m *= r5;
	m *= t2;
	frustum.applyTransformation(m);

	yztheta += theta;
}

void FixedPointCamera::rotateLR(float theta){
	Point eye = this->getEye();
	Point at = this->getAt();
	Matrix4x4 m(-at.x1,-at.x2,-at.x3);
	Matrix4x4 r1(-xytheta,Point(0,0,1));
	Matrix4x4 r2(-yztheta,Point(1,0,0));
	Matrix4x4 r3(yztheta,Point(1,0,0));
	Matrix4x4 r4(theta,Point(0,0,1));
	Matrix4x4 r5(xytheta,Point(0,0,1));
	Matrix4x4 t2(at.x1,at.x2,at.x3);
	m *= r1;
	m *= r2;
	m *= r3;
	m *= r4;
	m *= r5;
	m *= t2;
	frustum.applyTransformation(m);

	xytheta += theta;
}

void FixedPointCamera::setFixedPoint(Point p){
	Point distance=p-this->getAt();
	Matrix4x4 t(distance.x1,distance.x2,distance.x3);
	frustum.applyTransformation(t);
}

void FixedPointCamera::updatePosition(const Point& position,const Point& direction,PathInfo* info){
	this->setFixedPoint(position);
}
